
#include <math.h>



class KernelHMC{

public:
	KernelHMC(int d, mat F, mat g)
	{
		_d=d;
		_F=F;
		_g=-g;
		mat m(d,1);
		m.fill(0);
		_m=m;
		mat Sig(d,d);
		Sig.fill(1);
		_Sig=diagmat(Sig);
		boost::random::uniform_01<> U;
		G=new Distribution::Gaussian(d,_m,_Sig);
		runif=new RandomG::Random<boost::random::uniform_01<> >(U);
	}
	~KernelHMC()
	{
		delete runif;
	}
	//fonction qui calcul les uj Phij
	void  Phiu(int d, mat a, mat b, mat f, mat *phi, mat *u,mat g)
	{
		double s1=0;
		double s2=0;
		for(int j=0;j<d;j++)
		{
			s1=dot(f(j,span::all),a);				
			s2=dot(f(j,span::all),b);				
			(*u)(j,0)=sqrt(s1*s1+s2*s2);
			if((*u)(j,0)>abs<double>(g(0,j)))
			{
				(*phi)(j,0)=atan(-s1/s2);
				//cout << (*phi)(j,0) << " ";
			}else{
				(*phi)(j,0)=0;
			}
		}

	}

	mat Solveint(int d, mat g, mat phi, mat u)
	{
		mat sol(d,1);
		for(int j=0;j<d;j++)
		{
			if(u(j,0)>abs<double>(g(0,j)))
			{
				sol(j,0)=acos(-g(0,j)/u(j,0))-phi(j,0);
			}else{
				sol(j,0)=_T;
			}
		}
		return sol;
	}

	inline mat Kj(int t,mat phi, mat u, int d, mat g)
	{
		mat Kj(d,1);
		for(int j=0;j<d;j++)
		{
			Kj(j,0)=u(j,0)*cos(t+phi(j,0))+g(j,0);
		}
		return Kj;
	}

	inline double Xprocess(double t, double a, double b)
	{
		double x=a*sin(t)+b*cos(t);
		return x;
	}
	inline double Xpprocess(double t, double a, double b)
	{
		double x=a*cos(t)-b*sin(t);
		return x;
	}



	 void  Step(mat *xi, int d, double T, mat f, mat g)
	{
		mat a(d,1);
		mat b(d,1);
		int c=1;
		mat u(d,1);
		mat phi(d,1);
	
		mat temp=(*G).r(1);
		mat S=temp(span(0,d-1),0);
		//cout << S.t();
		double acct=0;
		while(c)
		{
			a=S;
			b=(*xi).t();
			Phiu(d,a,b,f,&phi,&u,g);
			mat t=Solveint(d,g,phi,u);
			int j=minvector(t);	
			//cout << "j " << j;
			if(t(j,0)<_T)
			{
				for(int i=0;i<d;i++)
				{
					(*xi)(0,i)=Xprocess(t(j,0),a(i,0),b(i,0));
					S(i,0)=Xpprocess(t(j,0),a(i,0),b(i,0));
				}
				//double norm2=dot(f(j,span::all),f(j,span::all));
				double alpha=as_scalar(dot(f(j,span::all),S)/_Norm(j,0));//norm2);
				S+=-2*alpha*f(j,span::all).t();
				//New S
				acct+=t(j,0);
				_T=_T-t(j,0);
				
			}else{
				for(int i=0;i<d;i++)
				{
					(*xi)(0,i)=Xprocess(_T,a(i,0),b(i,0));
				}
				c=0;
			}
		}
	}

	void Move(mat *X, double gamma)
	{
		int M=(*X).n_rows;
		mat g=_g-log(gamma);
		double T=(*runif)()*PI;
		_T=T;
		cout << "T " << T;
		mat f=_F;
		_Norm.resize(_d,1);
		for(int i=0;i<_d;i++)
		{
			cout << i;
			_Norm(i,0)=dot(f(i,span::all),f(i,span::all));
		}
		for(int i=0;i<M;i++){
			mat temp=(*X).row(i);
			Step(&temp,_d,T,_F,g);
			(*X).row(i)=temp;
		}
	}

	void Move2(mat *X, int _i)
	{
		int M=(*X).n_rows;
		_d=_i+1;
		double T=(*runif)()*PI;
		//T=0.05;
		_T=T;
		mat Y=(*X)(span::all,span(0,_i));
		cout << "T " << T;
		cout << _i;
		mat temp;
		mat f=_F(span(0,_i),span(0,_i));
		_Norm.resize(_d,1);
		for(int i=0;i<_d;i++)
		{
			_Norm(i,0)=dot(f(i,span::all),f(i,span::all));
		}
		mat g=_g(0,span(0,_i));
		for(int i=0;i<M;i++){
			temp=(Y).row(i);
			Step(&temp,_d,T,f,g);
			Y.row(i)=temp;
		}
		(*X)(span::all,span(0,_i))=Y;
	}

	private:
	int _d;
	mat _F;
	mat _g;
	mat _m;
	mat _Norm;
	mat _Sig;
	 Distribution::Gaussian *G;
	double _T;
	RandomG::Random<boost::random::uniform_01<> > *runif;

};

